---@diagnostic disable: undefined-global
import("System")
import("System.IO")
import("System.IO.Ports")
import("Jodell.Tool", "Jodell.Tool")

if(Directory.Exists("./ForceCtrl")~=true) then
	Info("创建路径")
	Directory.CreateDirectory("./ForceCtrl");
end
--打开串口
COMID = "COM9"
-- 定义设备ID对象
Device = {slave1 = 1,slave2 = 2,slave3 = 3,slave4 = 4,slave5 = 5}
-- 定义传感器ID对象
Sensor = {slave1 = 95,slave2 = 96,slave3 = 97,slave4 = 98,slave5 = 99}
-- 定义设备SN码对象
SN = {}
-- 定义数据文件操作对象
File_SN = {}
-- 定义数据文件路径对象
FilePath = {}
-- 定义验证文件操作对象
FileValidate = {}
-- 定义电流参考对象
CurRef = {}
-- 定义电流反馈对象
CurFbk = {}
-- 定义压力反馈对象
ForceFbk = {}
PullCur = 1000 --拉电流
PullFor = 100 --拉力
I_min = 200 --标定电流最小值 mA 
I_max = 1400 --标定电流最大值mA
F_min = 30 --验证力最小值N
F_max = 100 --验证力最大值N
client=Lua_ModbusRtuClient()
client.PortName=COMID
client:Connect()


-- 创建 CSV 对象并打开
-- 设备 SN读取
for i = 1,5,1 do
	ret,SN[i] = client:ReadString(Device[i],562,16)
	index=string.find(SN[i],"\0")
	SN[i]=string.sub(SN[i],1,index-1)
	Info(string.format("设备编号%s",SN[i]))
	FilePath[i] = string.format("./ForceCtrl\\推力测试(%s).csv",SN[i])
	Info(localpath)
	File_SN[i] = Lua_CsvWriter(localpath)

	File_SN[i]:Open()
	SN = 0
end

-- 写文件
for i = 1,5,1 do
	File_SN[i]:WriteString("参考电流(A)")
	File_SN[i]:WriteSplitter()
	File_SN[i]:WriteString("反馈电流(A)")
	File_SN[i]:WriteSplitter()
	File_SN[i]:WriteString("测量压力(N)")
	File_SN[i]:WriteSplitter()
	File_SN[i]:WriteString("设置电流(mA)")
	File_SN[i]:WriteSplitter()
	File_SN[i]:WriteLine()
end

--设备使能
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1002,1) 	
	if ret ~= 0 then
		Error(string.format("设置设备%d使能失败",i))
		goto exit
	end
end

--设备复位
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1004,1)	
	if ret ~= 0 then
		Error(string.format("设置设备%d复位失败",i))
		goto exit
	end
end

--切换到标定模式
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],0x013d,1)
	if ret ~= 0 then
		Error(string.format("设置设备%d标定模式失败",i))
		goto exit
	end
end

Delay(100)

for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1011,PullCur)	
	if ret == 0 then
		Info(string.format("设置拉电流:%dmA,设备%d",PullCur,i))
	else
		Error(string.format("设置拉电流:%dmA失败,设备%d",PullCur,i))
		goto exit
	end
end

--开始测试
for torque = I_min, I_max, 10 do
	--终止测试
    	if (IsCancel) then
    		Warn("手动终止")
            goto exit
		break
	end
	--读取运行状态
	--ret,runstatus = client:ReadUInt16(1,2000)
	--if ret ~= 0 or runstatus ~= 4 then
		--Error("切换运行模式失败")
		--goto exit
	--end
	--设置推电流
	for i = 1,5,1 do
		ret = client:WriteUInt16(Device[i],1010,torque)	
		if ret == 0 then
			Info(string.format("设置推电流:%dmA,设备%d",torque,i))
		else
			Error(string.format("设置推电流:%dmA失败,设备%d",torque,i))
			goto exit
		end
	end
	--设置速度
	
	--设置推动作
	for i = 1,5,1 do
		ret = client:WriteUInt16(Device[i],1003,1)	
		if ret ~= 0 then
			Error(string.format("设置设备%d推动作失败",i))
			goto exit
		end
	end

	
	Delay(1500)
	--读电流参考
	for i = 1,5,1 do
		ret,CurRef[i] = client:Read03FloatCDAB(Device[i],114)
		if ret ~= 0 then
			Error(string.format("读设备%d电流参考失败",i))
			goto exit
		end
		Info(string.format("推电流参考:%fA,设备%d",CurRef[i],i))
	end

	
	--读电流反馈
	for i = 1, 5, 1 do
		ret,CurFbk[i] = client:Read03FloatCDAB(Device[i],126)
		if ret ~= 0 then
			Error("读设备%d电流反馈失败",i)
			goto exit
		end
		Info(string.format("推电流反馈:%fA,设备%d",CurFbk[i],i))
	end
	--读压力
	for i = 1, 5, 1 do
		ret,ForceFbk[i] = client:Read04FloatABCD(Sensor[i],0)
		if ret ~= 0 then
			ret,ForceFbk[i] = client:Read04FloatABCD(Sensor[i],0)
			if ret ~= 0 then
				ret,ForceFbk[i] = client:Read04FloatABCD(Sensor[i],0)
				if ret ~= 0 then
					Error(string.format("读设备%d压力测量失败",i))
					goto exit
				end
			end
		end
		ForceFbk[i] = ForceFbk[i] * 9.8
		Info(string.format("测量压力:%fN,设备%d",ForceFbk[i],i))
	end
	
	for i = 1,5,1 do
		File_SN[i]:WriteFloat(-CurRef[i])
		File_SN[i]:WriteSplitter()
		File_SN[i]:WriteFloat(-CurFbk[i])
		File_SN[i]:WriteSplitter()
		File_SN[i]:WriteFloat(ForceFbk[i])
		File_SN[i]:WriteSplitter()
		File_SN[i]:WriteFloat(torque)
		File_SN[i]:WriteSplitter()
		File_SN[i]:WriteLine()
	end
	Delay(100)
	--设置拉动作
	for i = 1,5,1 do
		ret = client:WriteUInt16(Device[i],1003,0)	
		if ret ~= 0 then
			Error(string.format("设置设备%d拉动作失败",i))
			goto exit
		end
	end
	
	Delay(1000)
end

--关闭CSV文件
for i = 1,5,1 do
	File_SN[i]:Close()
end

client:Disconnect()

SalveNo = Device --Modbus从站号
PortName = COMID --串口名称
xIndex = 0 --电流 在文件中第几列，下标从0开始
yIndex = 2 --实际力 在文件中第几列，下标从0开始

pso_client = Lua_PsoClient() --线性拟合操作对象
client = Lua_ModbusRtuClient() --Modbus通讯对象
for i = 1,5,1 do
	pso_client:LoadCsvData(FilePath[i], xIndex, yIndex) --加载数据
	pso_client:Optimize(1000) --迭代计算分割点
	pso_client:FitData() --线性拟合
	if IsCancel then
		return
	end
	--打印拟合信息
	Info(string.format("设备%d共 %d 个分割点", i, pso_client.LineSegments.Count))
	for i = 0, pso_client.LineSegments.Count - 1 do
		Info(string.format("K:%f \tB:%f \tMinValue:%f \tMaxValue:%f",
			pso_client.LineSegments[i].K,
			pso_client.LineSegments[i].B,
			pso_client.LineSegments[i].MinValue,
			pso_client.LineSegments[i].MaxValue))
		if IsCancel then
			return
		end
	end
	--Modbus从站连接
	client.PortName = PortName
	client:Connect()
	if IsCancel then
		return
	end
	-- 写入拟合数据
	client:WriteUInt16(Device[i], 446, pso_client.LineSegments.Count)                      --下发分段数
	client:WriteFloatCDAB(Device[i], 448, pso_client.LineSegments[0].MinValue)             --下发上边界
	for i = 0, pso_client.LineSegments.Count - 1 do
		client:WriteFloatCDAB(Device[i], 450 + i * 2, pso_client.LineSegments[i].MaxValue) --下发分割点
		client:WriteFloatCDAB(Device[i], 460 + i * 2, pso_client.LineSegments[i].K)
		client:WriteFloatCDAB(Device[i], 470 + i * 2, pso_client.LineSegments[i].B)

		if IsCancel then
			return
		end
	end
	client:WriteUInt16(Device[i], 0x012C, 1) --保存参数
	Info(string.format"设备%d校准完成",i);
	pso_client:SaveImage("./data.png")
end

client:Disconnect()



--力控验证
Info("开始验证")
-- 创建文件
for i = 1,5,1 do
	FileValidate[i] = Lua_CsvWriter(string.format("./ForceCtrl/验证结果(%s).csv",SN[i]))
	FileValidate[i]:Open()
end

for i = 1,5,1 do
	FileValidate[i]:WriteString("参考电流(A)")
	FileValidate[i]:WriteSplitter()
	FileValidate[i]:WriteString("反馈电流(A)")
	FileValidate[i]:WriteSplitter()
	FileValidate[i]:WriteString("测量压力(N)")
	FileValidate[i]:WriteSplitter()
	FileValidate[i]:WriteString("设置压力(N)")
	FileValidate[i]:WriteSplitter()
	FileValidate[i]:WriteLine()
end 


client=Lua_ModbusRtuClient()
client.PortName=COMID
client:Connect()

--设备重启
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1000,1)
	
end

Delay(2000)

--设备使能
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1002,1)	
	if ret ~= 0 then
		Error(string.format("设置设备%d使能失败",i))
		goto exit
	end
end

--设备复位
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1004,1)	
	if ret ~= 0 then
		Error(string.format("设置设备%d复位失败",i))
		goto exit
	end
end

--切换到力控模式
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],0x013d,0)
	if ret ~= 0 then
		Error(string.format("设置设备%d力控模式失败",i))
		goto exit
	end
end

Delay(100)
for i = 1,5,1 do
	ret = client:WriteUInt16(Device[i],1011,100)	
	if ret == 0 then
		Info(string.format("设置设备%d拉力:%dN",i,PullFor))
	else
		Error(string.format("设置设备%d拉力:%dN失败",i,PullFor))
		goto exit
	end
end

--开始测试
for torque = F_min, F_max, 1 do
	--终止测试
    	if (IsCancel) then
    		Warn("手动终止")
            goto exit
		break
	end
	--设置推电流
	for i = 1,5,1 do
		ret = client:WriteUInt16(Device[i],1010,torque)	
		if ret == 0 then
			Info(string.format("设置推力:%dN,设备%d",torque,i))
		else
			Error(string.format("设置推力:%dN失败,设备%d",torque,i))
			goto exit
		end
	end 
	
	--设置推动作
	for i = 1,5,1 do
		ret = client:WriteUInt16(Device[i],1003,1)	
		if ret ~= 0 then
			Error(string.format("设置推动作失败,设备%d",i))
			goto exit
		end
	end
	
	Delay(1500)
	--读电流参考
	for i = 1,5,1 do
		ret,CurRef[i] = client:Read03FloatCDAB(Device[i],114)
		if ret ~= 0 then
			Error(string.format("读设备%d电流参考失败",i))
			goto exit
		end
		Info(string.format("推电流参考:%fA,设备%d",torque_ref,i))
	end
	
	--读电流反馈
	for i = 1,5,1 do
		ret,CurFbk[i] = client:Read03FloatCDAB(Device[i],126)
		if ret ~= 0 then
			Error(string.format("读设备%d电流反馈失败",i))
			goto exit
		end
		Info(string.format("推电流反馈:%fA,设备%d",torque_fbk,i))
	end
	
	--读压力
	for i = 1,5,1 do
		ret,ForceFbk[i] = client:Read04FloatABCD(Sensor[i],0)
		if ret ~= 0 then
			ret,ForceFbk[i] = client:Read04FloatABCD(Sensor[i],0)
			if ret ~= 0 then
				ret,ForceFbk[i] = client:Read04FloatABCD(Sensor[i],0)
				if ret ~= 0 then
					Error("读设备%d压力测量失败",i)
					goto exit
				end
			end
		end
		ForceFbk[i] = ForceFbk[i] * 9.8
		Info(string.format("测量压力:%fN,设备%d",ForceFbk[i]))
	end
	
	for i = 1,5,1 do
		FileValidate[i]:WriteFloat(-CurRef[i])
		FileValidate[i]:WriteSplitter()
		FileValidate[i]:WriteFloat(-CurFbk[i])
		FileValidate[i]:WriteSplitter()
		FileValidate[i]:WriteFloat(ForceFbk[i])
		FileValidate[i]:WriteSplitter()
		FileValidate[i]:WriteFloat(torque)
		FileValidate[i]:WriteSplitter()
		FileValidate[i]:WriteLine()
	end
   
	Delay(100)
	for i = 1,5,1 do
		ret = client:WriteUInt16(Device[i],1003,0)	
		if ret ~= 0 then
			Error(string.format("设置设备%d拉动作失败",i))
			goto exit
		end
	end
    
	Delay(1000)
end

--退出
::exit:: 
--关闭CSV文件
writer:Close()
--关闭串口
client:Disconnect()
Info("测试结束")
Info("打开文件查看结果")